#ifndef URDFPARSER_H
#define URDFPARSER_H

#include <QString>
#include <QVector3D>
#include <QMatrix4x4>
#include <QDomDocument>
#include <QVector>
#include <QObject>

struct Joint {
    QString name;
    QString type;
    QString parent;
    QString child;
    QVector3D origin;
    QVector3D axis;
    double limit_lower;
    double limit_upper;
    double position;
};

struct Link {
    QString name;
    QString mesh_path;
    QVector3D visual_origin;
    QMatrix4x4 transform;
    QVector3D color;
    QMatrix4x4 visual_transform;
};

class UrdfParser : public QObject
{
    Q_OBJECT

public:
    explicit UrdfParser(QObject *parent = nullptr);
    bool parseFile(const QString& filename);
    const QVector<Joint>& getJoints() const { return m_joints; }
    const QVector<Link>& getLinks() const { return m_links; }
    void updateJointPosition(const QString& name, double position);
    void setPackagePath(const QString& path) { m_packagePath = path; }

    QString strState() const;
signals:
    void modelLoaded();
    void jointUpdated();

private:
    bool parseJoint(const QDomElement& element);
    bool parseLink(const QDomElement& element);
    void updateTransforms();
    QString resolvePackagePath(const QString& uri);

    QVector<Joint> m_joints;
    QVector<Link> m_links;
    QMap<QString, int> m_jointMap;
    QString m_packagePath;

    QString m_strState;

};

#endif // URDFPARSER_H
